#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include <os.h>

#include "bsp.h"
#include "cmsis_os2.h"

/* Kernel version and identification string definition */
#define API_VERSION			2
#define KERNEL_VERSION		OS_VERSION
#define KERNEL_ID			"ucos-iii"

#ifndef UNUSED
#define UNUSED(var) do { (void)var; } while(0)
#endif

typedef struct _message_cb
{
	OS_MEM stOSMem;
	OS_Q stOSQueue;
} MSG_CB_S;

//  ==== Kernel Management Functions ====
osStatus_t osKernelInitialize(void)
{
	OS_ERR status;

	OSInit(&status);
	if (OS_ERR_NONE == status)
	{
		return osOK;
	}
	else
	{
		return osError;
	}
}

osStatus_t osKernelGetInfo(osVersion_t *version, char *id_buf, uint32_t id_size)
{
	if (version != NULL)
	{
		version->api = API_VERSION;
		version->kernel = KERNEL_VERSION;
	}

	if ((id_buf != NULL) && (id_size != 0U))
	{
		if (id_size > sizeof(KERNEL_ID))
		{
			id_size = sizeof(KERNEL_ID);
		}
		memcpy(id_buf, KERNEL_ID, id_size);
	}

	return osOK;
}

osKernelState_t osKernelGetState(void)
{
	if (DEF_TRUE == OSInitialized)
	{
		return osKernelInactive;
	}

	if (!OSRunning)
	{
		return osKernelReady;
	}
	else if (OSSchedLockNestingCtr > 0)
	{
		return osKernelLocked;
	}
	else
	{
		return osKernelRunning;
	}
}

osStatus_t osKernelStart(void)
{
	OS_ERR status;
	
	OSStart(&status);
	if (OS_ERR_NONE == status)
	{
		return osOK;
	}
	else
	{
		return osError;
	}
}

int32_t osKernelLock(void)
{
	OS_ERR status;

	OSSchedLock(&status);
	if (OS_ERR_NONE != status)
	{
		return (int32_t)osError;
	}

	return 1;
}

int32_t osKernelUnlock (void)
{
	int32_t lock;
	OS_ERR status;

	OSSchedUnlock(&status);
	if (OS_ERR_SCHED_LOCKED == status)
	{
		lock = 1;
	}
	else
	{
		lock = 0;
	}

	return lock;
}

int32_t osKernelRestoreLock (int32_t lock)
{
	switch (lock)
	{
	case 0:
		if (0 != osKernelUnlock())
		{
			break;
		}
		return 0;
	case 1:
		osKernelLock();
		return 1;
	default:
		break;
	}

	return (int32_t)osError;

}

uint64_t osKernelGetTickCount (void)
{
	uint64_t ticks;
#if (OS_VERSION >= 30000u)
	OS_ERR os_err;


	ticks = OSTimeGet(&os_err);
#else
	ticks = OSTimeGet();
#endif
	return ticks;
}

uint64_t osKernelGetTick2ms(void)
{
	uint32_t clk_freq = BSP_ClkFreqGet(BSP_CLK_ID_HCLK);
	return osKernelGetTickCount() * (1000 / clk_freq);
}

uint64_t osMs2Tick(uint64_t ms)
{
	uint32_t clk_freq = BSP_ClkFreqGet(BSP_CLK_ID_HCLK);
	return ms * (clk_freq / 1000);
}

uint32_t osKernelGetTickFreq (void)
{
	return BSP_ClkFreqGet(BSP_CLK_ID_HCLK);
}

uint32_t osKernelGetSysTimerCount (void)
{
	uint32_t count_low = 0;
	return count_low;
}

uint32_t osKernelGetSysTimerFreq (void)
{
	return 0;
}

//  ==== Thread Management Functions ====

osThreadId_t osThreadNew(osThreadFunc_t func, void *argument, const osThreadAttr_t *attr)
{
	OS_ERR err;
	OS_TCB *task_cb = NULL;
	OS_PRIO priority = 0;

	if ((attr == NULL)
		|| (func == NULL)
		|| (attr->priority < osPriorityLow1)
		|| (attr->priority > osPriorityAboveNormal7))
	{
		return (osThreadId_t)NULL;
	}

	task_cb = (OS_TCB *)malloc(sizeof(OS_TCB));
	if (!task_cb)
	{
		return (osThreadId_t)NULL;
	}

	memset(task_cb, 0, sizeof(OS_TCB));
	priority   = OS_CFG_PRIO_MAX - (attr->priority - osPriorityLow); /*0~31*/

	OSTaskCreate(task_cb,
				(CPU_CHAR *)(attr->name),
				func,
				argument,
				priority,
				attr->stack_mem,
				attr->stack_size/10,
				attr->stack_size,
				attr->reserved,
				0,
				NULL,
				(OS_OPT)(OS_OPT_TASK_STK_CHK | OS_OPT_TASK_STK_CLR|OS_OPT_TASK_SAVE_FP),
				&err);

	if (OS_ERR_NONE != err)
	{
		free(task_cb);
		return (osThreadId_t)NULL;
	}

	return (osThreadId_t)task_cb;
}

const char *osThreadGetName(osThreadId_t thread_id)
{
	OS_TCB *task_cb = (OS_TCB *)thread_id;
	if (!task_cb || !task_cb->NamePtr)
	{
		return "unknow";
	}
	return task_cb->NamePtr;
}

osThreadId_t osThreadGetId(void)
{
	return (osThreadId_t)NULL;
}

osThreadState_t osThreadGetState(osThreadId_t thread_id)
{
	OS_STATE task_status;
	OS_TCB *task_cb = (OS_TCB *)thread_id;
	osThreadState_t thread_state;

	if (thread_id == NULL)
	{
		return osThreadError;
	}

	task_status = task_cb->TaskState;
	if (task_status & (OS_TASK_STATE_BIT_DLY | OS_TASK_STATE_BIT_PEND | OS_TASK_STATE_BIT_SUSPENDED))
	{
		thread_state = osThreadBlocked;
	}
	else if (OS_TASK_STATE_DEL == task_status)
	{
		thread_state = osThreadTerminated;
	}
	else
	{
		thread_state = osThreadReady;
	}

	return thread_state;
}

uint32_t osThreadGetStackSize(osThreadId_t thread_id)
{
	OS_TCB *task_cb = (OS_TCB *)thread_id;

	if (thread_id == NULL)
	{
		return 0U;
	}

	return task_cb->StkSize;
}

uint32_t osThreadGetStackSpace(osThreadId_t thread_id)
{
	CPU_STK_SIZE  free = 0;
	CPU_STK_SIZE  used = 0;
	OS_ERR err;
	OS_TCB *task_cb = (OS_TCB *)thread_id;

	if (thread_id == NULL)
	{
		return 0U;
	}

	OSTaskStkChk(task_cb, &free, &used, &err);
	return (uint32_t)free;
}

osStatus_t osThreadSetPriority(osThreadId_t thread_id, osPriority_t priority)
{
	OS_PRIO os_prio = 0;
	OS_ERR err;
	OS_TCB *task_cb = (OS_TCB *)thread_id;

	if (thread_id == NULL)
	{
		return osErrorParameter;
	}

	if (priority < osPriorityLow1 || priority > osPriorityAboveNormal7)
	{
		return osErrorParameter;
	}

	os_prio   = OS_CFG_PRIO_MAX - (priority - osPriorityLow); /*0~31*/
	OSTaskChangePrio(task_cb, os_prio, &err);
	switch (err)
	{
		case OS_ERR_STATE_INVALID:
		case OS_ERR_TASK_CHANGE_PRIO_ISR:
		case OS_ERR_PRIO_INVALID:
			return osErrorParameter;

		case OS_ERR_OS_NOT_RUNNING:
			return osErrorResource;

		case OS_ERR_NONE:
			return osOK;
		default:
			return osError;
	}
}

osPriority_t osThreadGetPriority(osThreadId_t thread_id)
{
	OS_TCB *task_cb = (OS_TCB *)thread_id;

	if (thread_id == NULL)
	{
		return osPriorityError;
	}

	return (osPriority_t)(OS_CFG_PRIO_MAX - (task_cb->Prio - osPriorityLow));
}

osStatus_t osThreadYield(void)
{
	return osErrorResource;
}

osStatus_t osThreadSuspend(osThreadId_t thread_id)
{
	OS_ERR err;
	OS_TCB *task_cb = (OS_TCB *)thread_id;

	if (thread_id == NULL)
	{
		return osErrorParameter;
	}

	OSTaskSuspend(task_cb, &err);
	switch (err)
	{
		case OS_ERR_STATE_INVALID:
			return osErrorParameter;

		case OS_ERR_TASK_SUSPEND_ISR:
		case OS_ERR_TASK_SUSPEND_IDLE:
		case OS_ERR_OS_NOT_RUNNING:
		case OS_ERR_SCHED_LOCKED:
			return osErrorResource;

		case OS_ERR_NONE:
			return osOK;
		default:
			return osError;
	}
}

osStatus_t osThreadResume(osThreadId_t thread_id)
{
	OS_ERR err;
	OS_TCB *task_cb = (OS_TCB *)thread_id;

	if (thread_id == NULL)
	{
		return osErrorParameter;
	}

	OSTaskResume(task_cb, &err);
	switch (err)
	{
		case OS_ERR_STATE_INVALID:
		case OS_ERR_TASK_RESUME_SELF:
			return osErrorParameter;

		case OS_ERR_OS_NOT_RUNNING:
		case OS_ERR_TASK_RESUME_ISR:
		case OS_ERR_TASK_NOT_SUSPENDED:
			return osErrorResource;

		case OS_ERR_NONE:
			return osOK;
		default:
			return osError;
	}
}

osStatus_t osThreadTerminate(osThreadId_t thread_id)
{
	OS_ERR err;
	OS_TCB *task_cb = (OS_TCB *)thread_id;

	if (thread_id == NULL)
	{
		return osErrorParameter;
	}

	OSTaskDel(task_cb, &err);
	free(task_cb);
	switch (err)
	{
		case OS_ERR_ILLEGAL_DEL_RUN_TIME:
		case OS_ERR_TASK_DEL_IDLE:
			return osErrorParameter;

		case OS_ERR_TASK_DEL_ISR:
		case OS_ERR_OS_NOT_RUNNING:
			return osErrorResource;

		case OS_ERR_NONE:
			return osOK;
		default:
			return osError;
	}
}

uint32_t osThreadGetCount(void)
{
	uint32_t uwCount = 0;
	return uwCount;
}

//  ==== Generic Wait Functions ====
osStatus_t osDelay (uint32_t ticks)
{
	OS_ERR err;

	OSTimeDly(ticks, OS_OPT_TIME_DLY, &err);
	if(OS_ERR_NONE == err)
	{
		return osOK;
	}
	else
	{
		return osError;
	}
}

osStatus_t osDelayUntil(uint64_t ticks)
{
	OS_ERR err;
	uint64_t delay_ticks;
	uint64_t tick_count = osKernelGetTickCount();

	if(ticks < tick_count)
	{
		return osError;
	}

	delay_ticks = (OS_TICK)(ticks - tick_count);
	OSTimeDly(delay_ticks, OS_OPT_TIME_DLY, &err);
	if(OS_ERR_NONE == err)
	{
		return osOK;
	}
	else
	{
		return osError;
	}
}

//  ==== Timer Management Functions ====
osTimerId_t osTimerNew(osTimerFunc_t func, osTimerType_t type, void *argument, const osTimerAttr_t *attr)
{
	OS_ERR err;
	OS_TMR *pTmr = NULL;
	OS_OPT mode;
	const char *name = NULL;

	if ((NULL == func) || ((osTimerOnce != type) && (osTimerPeriodic != type)))
	{
		return (osTimerId_t)NULL;
	}

	if (attr)
	{
		name = attr->name;
	}
	
	if(osTimerOnce == type)
	{
		mode = OS_OPT_TMR_ONE_SHOT;
	}
	else
	{
		mode = OS_OPT_TMR_PERIODIC;
	}

	pTmr = (OS_TMR *)malloc(sizeof(OS_TMR));
	OSTmrCreate(pTmr, (CPU_CHAR *)name, 1000, 1000, mode, (OS_TMR_CALLBACK_PTR)func, argument, &err);
	return (osTimerId_t)pTmr;
}

osStatus_t osTimerStart(osTimerId_t timer_id, uint32_t ticks)
{
	OS_ERR err;
	OS_TMR *sw_tmr = (OS_TMR *)timer_id;

	if ((0 == ticks) || (NULL == timer_id))
	{
		return osErrorParameter;
	}

	OSTmrSet(sw_tmr, ticks, ticks, sw_tmr->CallbackPtr, sw_tmr->CallbackPtrArg, &err);
	if (OS_ERR_NONE != err)
	{
		return osError;
	}

	OSTmrStart(sw_tmr, &err);
	if (OS_ERR_NONE != err)
	{
		return osError;
	}

	return osOK;
}

const char *osTimerGetName(osTimerId_t timer_id)
{
	OS_TMR *sw_tmr = (OS_TMR *)timer_id;
	return (const char *)sw_tmr->NamePtr;
}

osStatus_t osTimerStop(osTimerId_t timer_id)
{
	OS_ERR err;
	OS_TMR *sw_tmr = (OS_TMR *)timer_id;

	if (NULL == sw_tmr)
	{
		return osErrorParameter;
	}

	OSTmrStop(sw_tmr, OS_OPT_TMR_NONE, NULL, &err);
	if (OS_ERR_NONE == err)
	{
		return osOK;
	}
	return osError;
}


uint32_t osTimerIsRunning(osTimerId_t timer_id)
{
	OS_ERR err;
	OS_TMR *sw_tmr = (OS_TMR *)timer_id;
	OS_STATE state;
	
	if (NULL == timer_id)
	{
		return 0;
	}

	state = OSTmrStateGet(sw_tmr, &err);
	return (OS_TMR_STATE_RUNNING == state);
}

osStatus_t osTimerDelete(osTimerId_t timer_id)
{
	OS_ERR err;
	OS_TMR *sw_tmr = (OS_TMR *)timer_id;

	if (NULL == sw_tmr)
	{
		return osErrorParameter;
	}

	OSTmrDel(sw_tmr, &err);
	free(sw_tmr);
	if (OS_ERR_NONE == err)
	{
		return osOK;
	}
	else
	{
		return osError;
	}
}

osEventFlagsId_t osEventFlagsNew(const osEventFlagsAttr_t *attr)
{
	OS_ERR err;
	OS_FLAG_GRP *event_cb = NULL;
	const char *name = NULL;

	if (attr)
	{
		name = attr->name;
	}

	event_cb = (OS_FLAG_GRP *)malloc(sizeof(OS_FLAG_GRP));
	if(event_cb == NULL )
	{
		return (osEventFlagsId_t)NULL;
	}

	OSFlagCreate(event_cb, (CPU_CHAR *)name, 0, &err);
	if (OS_ERR_NONE == err)
	{
		free(event_cb);
		return (osEventFlagsId_t)NULL;
	}
	else
	{
		return (osEventFlagsId_t)event_cb;
	}
}

const char *osEventFlagsGetName(osEventFlagsId_t ef_id)
{
	OS_FLAG_GRP *event_cb = (OS_FLAG_GRP *)ef_id;

	if (!event_cb || !event_cb->NamePtr)
	{
		return "unknow";
	}

	return event_cb->NamePtr;
}

uint32_t osEventFlagsSet(osEventFlagsId_t ef_id, uint32_t flags)
{
	OS_ERR err;
	OS_FLAG_GRP *event_cb = (OS_FLAG_GRP *)ef_id;
	OS_FLAGS rflags;

	if (!event_cb)
	{
		return (uint32_t)osFlagsErrorParameter;
	}
	
	rflags = OSFlagPost(event_cb, OS_OPT_POST_FLAG_SET, flags, &err);
	if (err != OS_ERR_NONE)
	{
		return (uint32_t)osFlagsErrorParameter;
	}
	else
	{
		return (uint32_t)rflags;
	}
}

uint32_t osEventFlagsClear(osEventFlagsId_t ef_id, uint32_t flags)
{
	OS_ERR err;
	OS_FLAG_GRP *event_cb = (OS_FLAG_GRP *)ef_id;
	OS_FLAGS rflags;

	if (!event_cb)
	{
		return (uint32_t)osFlagsErrorParameter;
	}
	
	rflags = OSFlagPost(event_cb, OS_OPT_POST_FLAG_CLR, flags, &err);
	if (err != OS_ERR_NONE)
	{
		return (uint32_t)osFlagsErrorParameter;
	}
	else
	{
		return (uint32_t)rflags;
	}
}

uint32_t osEventFlagsGet(osEventFlagsId_t ef_id)
{
	OS_FLAG_GRP *event_cb = (OS_FLAG_GRP *)ef_id;

	if (!event_cb)
	{
		return (uint32_t)osFlagsErrorParameter;
	}

	return event_cb->Flags;
}

uint32_t osEventFlagsWait(osEventFlagsId_t ef_id, uint32_t flags, uint32_t options, uint32_t timeout)
{
	OS_ERR err;
	OS_FLAG_GRP *event_cb = (OS_FLAG_GRP *)ef_id;
	OS_OPT opt = 0;
	OS_FLAGS rflags;

	if (!event_cb)
	{
		return (uint32_t)osFlagsErrorParameter;
	}

	if (osNoWait == timeout)
	{
		opt |= OS_OPT_PEND_NON_BLOCKING;
	}
	else if (osWaitForever == timeout)
	{
		timeout = 0;
		opt |= OS_OPT_PEND_BLOCKING;
	}
	else
	{
		opt |= OS_OPT_PEND_BLOCKING;
	}
	
	if (osFlagsNoClear & options)
	{
		if (osFlagsWaitAny & options)
		{
			opt |= OS_OPT_PEND_FLAG_SET_ANY;
		}

		if (osFlagsWaitAll & options)
		{
			opt |= OS_OPT_PEND_FLAG_SET_ALL;
		}
	}
	else
	{
		if (osFlagsWaitAny & options)
		{
			opt |= OS_OPT_PEND_FLAG_CLR_ANY;
		}

		if (osFlagsWaitAll & options)
		{
			opt |= OS_OPT_PEND_FLAG_CLR_ALL;
		}
	}
	
	rflags = OSFlagPend(event_cb, flags, timeout, opt, NULL, &err);
	switch (err)
	{
		case OS_ERR_OBJ_PTR_NULL:
		case OS_ERR_OPT_INVALID:
		case OS_ERR_STATUS_INVALID:
			return (uint32_t)osFlagsErrorParameter;

		case OS_ERR_PEND_ISR:
		case OS_ERR_OS_NOT_RUNNING:
		case OS_ERR_SCHED_LOCKED:
		case OS_ERR_PEND_WOULD_BLOCK:
		case OS_ERR_PEND_ABORT:
			return (uint32_t)osFlagsErrorResource;

		case OS_ERR_TIMEOUT:
			return (uint32_t)osFlagsErrorTimeout;

		case OS_ERR_NONE :
			return (uint32_t)rflags;
		default:
			return (uint32_t)osFlagsErrorUnknown;
	}
}

osStatus_t osEventFlagsDelete(osEventFlagsId_t ef_id)
{
#if (OS_CFG_FLAG_DEL_EN == DEF_ENABLED)
	OS_ERR err;
	OS_FLAG_GRP *event_cb = (OS_FLAG_GRP *)ef_id;
	osStatus_t uwRet;

	if (!ef_id)
	{
		return osErrorParameter;
	}
	
	OSFlagDel(event_cb, OS_OPT_DEL_ALWAYS, &err);
	if (OS_ERR_NONE == err)
	{
		uwRet = osOK;
	}
	else
	{
		uwRet = osErrorParameter;
	}

	free(event_cb);
	return uwRet;
#else
	return osErrorResource;
#endif
}

//  ==== Mutex Management Functions ====
osMutexId_t osMutexNew(const osMutexAttr_t *attr)
{
	OS_ERR err;
	OS_MUTEX  *mutex = NULL;
	const char *pName = NULL;

	if (attr)
	{
		pName = attr->name;
	}

	mutex = (OS_MUTEX *)malloc(sizeof(OS_MUTEX));
	if (!mutex)
	{
		return (osMutexId_t)NULL;
	}
	
	OSMutexCreate(mutex, (CPU_CHAR *)pName, &err);
	if(OS_ERR_NONE == err)
	{
		return (osMutexId_t)(mutex);
	}
	else
	{
		return (osMutexId_t)NULL;
	}
}

osStatus_t osMutexAcquire(osMutexId_t mutex_id, uint32_t timeout)
{
	OS_ERR err;
	OS_MUTEX  *mutex = (OS_MUTEX  *)mutex_id;
	OS_OPT opt = OS_OPT_PEND_BLOCKING;

	if (mutex_id == NULL)
	{
		return osErrorParameter;
	}

	if (osNoWait == timeout)
	{
		opt = OS_OPT_PEND_NON_BLOCKING;
	}

	if (osWaitForever == timeout)
	{
		timeout = 0;
	}

	OSMutexPend(mutex, timeout, opt, NULL, &err);
	if (OS_ERR_NONE == err)
	{
		return osOK;
	}
	else if (OS_ERR_TIMEOUT == err)
	{
		return osErrorTimeout;
	}
	else if (OS_ERR_OBJ_PTR_NULL == err)
	{
		return osErrorParameter;
	}
	else
	{
		return osErrorResource;
	}
}

osStatus_t osMutexRelease(osMutexId_t mutex_id)
{
	OS_ERR err;
	OS_MUTEX  *mutex_cb = (OS_MUTEX  *)mutex_id;

	if (mutex_id == NULL)
	{
		return osErrorParameter;
	}

	OSMutexPost(mutex_cb, OS_OPT_POST_NONE, &err);
	if (OS_ERR_NONE == err)
	{
		return osOK;
	}
	else
	{
		return osErrorResource;
	}
}

osThreadId_t osMutexGetOwner(osMutexId_t mutex_id)
{
	OS_MUTEX  *mutex_cb = (OS_MUTEX  *)mutex_id;

	if (mutex_id == NULL)
	{
		return (osThreadId_t)NULL;
	}

	return (osThreadId_t)(mutex_cb->OwnerTCBPtr);
}

osStatus_t osMutexDelete(osMutexId_t mutex_id)
{
	OS_ERR err;
	OS_MUTEX  *mutex_cb = (OS_MUTEX  *)mutex_id;

	if (mutex_id == NULL)
	{
		return osErrorParameter;
	}

	OSMutexDel(mutex_cb, OS_OPT_DEL_ALWAYS, &err);
	free(mutex_cb);
	if (OS_ERR_NONE == err)
	{
		return osOK;
	}
	else if (OS_ERR_OBJ_TYPE == err)
	{
		return osErrorParameter;
	}
	else
	{
		return osErrorResource;
	}
}

//  ==== Semaphore Management Functions ====
osSemaphoreId_t osSemaphoreNew(uint32_t max_count, uint32_t initial_count, const osSemaphoreAttr_t *attr)
{
	UNUSED(max_count);

	OS_ERR err;
	OS_SEM  *semaphore = NULL;
	const char *name = NULL;

	if (attr)
	{
		name = attr->name;
	}

	semaphore = (OS_SEM *)malloc(sizeof(OS_SEM));
	OSSemCreate(semaphore, (CPU_CHAR *)name, initial_count, &err);
	if (OS_ERR_NONE == err)
	{
		return (osSemaphoreId_t)(semaphore);
	}
	else
	{
		free(semaphore);
		return (osSemaphoreId_t)NULL;
	}
}

osStatus_t osSemaphoreAcquire(osSemaphoreId_t semaphore_id, uint32_t timeout)
{
	OS_ERR err;
	OS_SEM  *semaphore_cb = (OS_SEM  *)semaphore_id;
	OS_OPT opt = OS_OPT_PEND_BLOCKING;
	
	if (semaphore_id == NULL)
	{
		return osErrorParameter;
	}

	if (osNoWait == timeout)
	{
		opt = OS_OPT_PEND_NON_BLOCKING;
	}

	if (osWaitForever == timeout)
	{
		timeout = 0;
	}

	OSSemPend(semaphore_cb, timeout, opt, NULL, &err);
	if (OS_ERR_NONE == err)
	{
		return osOK;
	}
	else if (OS_ERR_TIMEOUT == err)
	{
		return osErrorTimeout;
	}
	else if (OS_ERR_OPT_INVALID == err)
	{
		return osErrorParameter;
	}
	else if (OS_ERR_PEND_ISR == err)
	{
		return osErrorISR;
	}
	else
	{
		return osErrorResource;
	}
}

osStatus_t osSemaphoreRelease(osSemaphoreId_t semaphore_id)
{
	OS_ERR err;
	OS_SEM  *semaphore_cb = (OS_SEM  *)semaphore_id;

	if (semaphore_id == NULL)
	{
		return osErrorParameter;
	}

	OSSemPost(semaphore_cb, OS_OPT_POST_FIFO, &err);
	if (OS_ERR_NONE == err)
	{
		return osOK;
	}
	else if (OS_ERR_OBJ_TYPE == err)
	{
		return osErrorParameter;
	}
	else
	{
		return osErrorResource;
	}
}

uint32_t osSemaphoreGetCount(osSemaphoreId_t semaphore_id)
{
	return 0;
}

osStatus_t osSemaphoreDelete(osSemaphoreId_t semaphore_id)
{
	OS_ERR err;
	OS_SEM  *semaphore_cb = (OS_SEM  *)semaphore_id;

	if (semaphore_id == NULL)
	{
		return osErrorParameter;
	}

	OSSemDel(semaphore_cb, OS_OPT_DEL_ALWAYS, &err);
	free(semaphore_cb);
	if (OS_ERR_NONE == err)
	{
		return osOK;
	}
	else if (OS_ERR_OBJ_TYPE == err)
	{
		return osErrorParameter;
	}
	else
	{
		return osErrorResource;
	}
}

//  ==== Message Queue Management Functions ====
osMessageQueueId_t osMessageQueueNew(uint32_t msg_count, uint32_t msg_size, const osMessageQueueAttr_t *attr)
{
	OS_ERR err;
	MSG_CB_S  *msg_cb = NULL;

	if (0 == msg_count || 0 == msg_size || !attr || !attr->mq_mem)
	{
		return (osMessageQueueId_t)NULL;
	}

	msg_cb = (MSG_CB_S *)malloc(sizeof(MSG_CB_S));
	if (!msg_cb)
	{
		return (osMessageQueueId_t)NULL;
	}

	OSMemCreate(&msg_cb->stOSMem, (CPU_CHAR *)(attr->name), attr->mq_mem, msg_count, msg_size, &err);
	if (OS_ERR_NONE != err)
	{
		free(msg_cb);
		return (osMessageQueueId_t)NULL;
	}
	
	OSQCreate(&msg_cb->stOSQueue, (CPU_CHAR *)(attr->name), msg_count, &err);
	if (OS_ERR_NONE == err)
	{
		return (osMessageQueueId_t)(msg_cb);
	}
	else
	{
		free(msg_cb);
		return (osMessageQueueId_t)NULL;
	}
}

osStatus_t osMessageQueuePut(osMessageQueueId_t mq_id, const void *msg_ptr, uint8_t msg_prio, uint32_t timeout)
{
	OS_ERR err;
	MSG_CB_S  *msg_cb = (MSG_CB_S  *)mq_id;
	void *mem_blk = NULL;

	if (mq_id == NULL || msg_ptr == NULL)
	{
		return osErrorParameter;
	}

	mem_blk = OSMemGet(&msg_cb->stOSMem, &err);
	if (OS_ERR_NONE != err)
	{
		return osErrorResource;
	}

	memcpy(mem_blk, msg_ptr, msg_cb->stOSMem.BlkSize);
	OSQPost(&msg_cb->stOSQueue, mem_blk, msg_cb->stOSMem.BlkSize, OS_OPT_POST_FIFO, &err);
	if (OS_ERR_TIMEOUT == err)
	{
		OSMemPut(&msg_cb->stOSMem, mem_blk, &err);
		return osErrorTimeout;
	}
	else if (OS_ERR_NONE != err)
	{
		OSMemPut(&msg_cb->stOSMem, mem_blk, &err);
		return osErrorResource;
	}
	else
	{
		return osOK;
	}
}

osStatus_t osMessageQueueGet(osMessageQueueId_t mq_id, void *msg_ptr, uint8_t *msg_prio, uint32_t timeout)
{
	OS_ERR err;
	MSG_CB_S  *msg_cb = (MSG_CB_S  *)mq_id;
	OS_OPT opt = OS_OPT_PEND_BLOCKING;
	OS_MSG_SIZE msg_size = 0;
	void *mem_blk = NULL;

	if (mq_id == NULL || msg_ptr == NULL)
	{
		return osErrorParameter;
	}
	
	if (osNoWait == timeout)
	{
		opt = OS_OPT_PEND_NON_BLOCKING;
	}

	if (osWaitForever == timeout)
	{
		timeout = 0;
	}

	mem_blk = OSQPend(&msg_cb->stOSQueue, timeout, opt, &msg_size, NULL, &err);
	if (OS_ERR_NONE != err)
	{
		return osErrorResource;
	}

	memcpy(msg_ptr, mem_blk, msg_cb->stOSMem.BlkSize);
	OSMemPut(&msg_cb->stOSMem, mem_blk, &err);
	return osOK;
}

uint32_t osMessageQueueGetCapacity(osMessageQueueId_t mq_id)
{
	MSG_CB_S  *msg_cb = (MSG_CB_S  *)mq_id;

	if (msg_cb == NULL)
	{
		return 0;
	}

	return (uint32_t)(msg_cb->stOSMem.NbrMax);
}

uint32_t osMessageQueueGetMsgSize(osMessageQueueId_t mq_id)
{
	MSG_CB_S  *msg_cb = (MSG_CB_S  *)mq_id;

	if (msg_cb == NULL)
	{
		return 0;
	}
	
	return (uint32_t)(msg_cb->stOSMem.NbrMax * msg_cb->stOSMem.BlkSize);
}

uint32_t osMessageQueueGetCount(osMessageQueueId_t mq_id)
{
	MSG_CB_S  *msg_cb = (MSG_CB_S  *)mq_id;

	if (msg_cb == NULL)
	{
		return 0U;
	}

	return (uint32_t)(msg_cb->stOSMem.NbrMax - msg_cb->stOSMem.NbrFree);
}

uint32_t osMessageQueueGetSpace(osMessageQueueId_t mq_id)
{
	MSG_CB_S  *msg_cb = (MSG_CB_S  *)mq_id;

	if (msg_cb == NULL)
	{
		return 0U;
	}

	return (uint32_t)(msg_cb->stOSMem.NbrFree);
}

osStatus_t osMessageQueueDelete(osMessageQueueId_t mq_id)
{
#if (OS_CFG_Q_DEL_EN == DEF_ENABLED)
	OS_ERR err;
	MSG_CB_S *msg_cb = (MSG_CB_S  *)mq_id;

	if (msg_cb == NULL)
	{
		return osErrorParameter;
	}

	OSQDel(&msg_cb->stOSQueue, OS_OPT_DEL_ALWAYS, &err);
	free(msg_cb);
	if (OS_ERR_NONE == err)
	{
		return osOK;
	}
	else if (OS_ERR_OBJ_TYPE == err || OS_ERR_OPT_INVALID == err)
	{
		return osErrorParameter;
	}
	else
	{
		return osErrorResource;
	}
#else
	return osErrorResource;
#endif
}


